﻿//这里对之前写的A-star 算法进行优化
// 在网络拓扑节点是 比较连续的情况下 使用 vector ，支持随机访问，节省查找时间

//这个文件的数据 是针对2011 广州的数据
# include"common.h"


//  采用vector 的方式存储节点，这样可以更快的查找
// 
class AStarEngine2
{
public:

	AStarEngine2()
	{
		CountNode = 180000;
		NodeInfoMap.resize(CountNode);
		NodeLinkInfoMap.resize(CountNode);
		RoadInfoMap.resize(CountNode + 50000);
		//startMapRoad.resize(CountNode);
		getNodeInfo();
		getLocationInfo();
	}
	
	//得到要道路拓扑节点的信息
	
	list<NodeInfo> Execute(NodeInfo startnode ,NodeInfo endnode, int Condition)
	{

	}
	void Solve(int starti,int endi)
	{
	
		if( false == check(starti) || false == check(endi) )
		{
			cout << " the id is not in map"<<endl;
			return ;
		}

		StartNode = starti;EndNode  = endi;

		bool noFindFlag = true;
		int p = starti;
		double cost = getCost(p,endi);
		CostNode costnode(p,0,cost);
		
		
		int count = 0;
		init();
		//首先要把startnode 放到close 集合中
		//closeSet.insert(starti);
		openSet.insert(map<int,CostNode>::value_type(p,costnode));
		ParentNode[p] = p;
		int nStep = 0;
		while(noFindFlag)
		{
			if( openSet.empty() )
			{
				break;
			}

			int besti =getBest();
			nStep++;
			map<int ,CostNode>::iterator iter = openSet.find(besti);
			closeSet.insert(map<int ,CostNode >::value_type(besti,iter->second) );
			if(isTarget(besti,endi) )
			{
				noFindFlag =  false;
			}
#ifdef CPMTIME
			cout <<"Num="<<nStep<< " BestNode = " << besti << endl ;
#endif
			extendRoad(besti);
			
			if(openSet.find(besti) != openSet.end())
				openSet.erase(besti);//
			count ++;

		}
#ifdef CPMTIME
		cout << "A2* :" <<endl;
		if(noFindFlag == false)
		{
			cout << "Yes you can get  : " << count << endl;
			makePath(EndNode);
			PrintPath();

		}
		else 
		{
			cout << "can not get it : "<< count  << endl;
		}
#endif
		


	}
	void PrintPath()
	{
		list<int>::iterator iter = Path.begin();

		cout << "The Path is : " ;
		for(;  iter != Path.end() ;iter ++)
		{
			cout << "   " << *iter ;
		}
		cout << endl;

		cout << "the road path is :"  ;
		for(iter = RoadPath.begin();  iter != RoadPath.end() ;iter ++)
		{
			cout << "   " << *iter ;
		}
		cout << endl;
		
		cout << "打印用于查询的sql 语句" <<endl  ;
		for(iter = RoadPath.begin();  iter != RoadPath.end() ;iter ++)
		{
			cout << " ROAD_ = " << *iter << " or" ;
		}
		cout << endl;

		cout << "打印计算了哪些点" <<endl  ;

		for(map<int,CostNode>::iterator iter = openSet.begin() ; iter != openSet.end() ; iter ++)
		{
			cout << " FNODE_ = " << iter->first << " or" ;
		}

		for(map<int,CostNode>::iterator iter = closeSet.begin() ; iter != closeSet.end() ; iter ++)
		{
			cout << " FNODE_ = " << iter->first << " or" ;
		}

		cout << endl;

	}
 
	

private:
	//map< int ,list<int> > NodeLinkInfoMap;
	//map< int ,NodeInfo>   NodeInfoMap;

	vector< LinkNode > NodeLinkInfoMap;
	vector< NodeInfo> NodeInfoMap;
	vector<NodeEntityInfo >RoadInfoMap;
	map< int ,CostNode> openSet;
	vector<int > ParentNode;
	list<int > Path;
	list<int> RoadPath;
	int StartNode ,EndNode,CountNode;
	double  maxCost; //最大的花费
	double maxLen;// 最大的路径长度
	//map< int ,CostNode> closeSet;
	map<int ,CostNode> closeSet;


#ifdef YOUHUA1
	double weigh ;
	double maxX;
	double maxY ; 
	double minX ;
	double minY ;
#endif
	
/*	struct compare
	{
		bool operator()(const int s1,const int s2)const{
			map<int ,CostNode>::iterator iter1 = openSet.find(s1);
			map<int ,CostNode>::iterator iter2 = openSet.find(s2);

			return iter1->second.sum -  iter2->second.sum;
		}
	}*/;

	void getNodeInfo()
	{
		int isnode ,ienode;
		double lenth;
		int roadId;
		char *filename = "G-data-road.txt";
		ifstream inputfile(filename);
		if(!inputfile)
		{
			cout<< "file open error" <<endl;
		}
		while(inputfile >>roadId >>  isnode  >> ienode >>lenth )
		{
			//vector< list<int> > ::iterator iter = NodeLinkInfoMap.find(isnode);

			NodeLinkInfoMap[isnode].count ++;
			
			NodeLinkInfoMap[isnode].link.push_back(ienode);
			

			////这里暂且认为 道路是双连通的 即是一个无向图
			//NodeLinkInfoMap[ienode].count ++;

			//NodeLinkInfoMap[ienode].link.push_back(isnode);
			//
			//  对RoadInfoMap 进行赋值
			RoadInfoMap[roadId].StartId =  isnode;
			RoadInfoMap[roadId].EndId = ienode;
			RoadInfoMap[roadId].Length = lenth;
			RoadInfoMap[roadId].Direct = 0;


#ifdef CPMTIME

			//startMapRoad[isnode] = roadId;
			char str[20] = {NULL};
			sprintf(str,"%d+%d",isnode,ienode);
			string strRoad(str);
			
			map<string ,int >::iterator mapiter;
			mapiter = startMapRoad.find(strRoad);
			if(mapiter  == startMapRoad.end())
			{
				startMapRoad.insert(map<string,int >::value_type(strRoad,roadId));
			}

#endif


		}

		inputfile.close();

	}


	//得到每个节点的坐标
	void getLocationInfo()
	{
		double  x,y;
		int i;
		ifstream inputfile("G-location.txt");
		if(!inputfile)
		{
			cout << "file  open  error" << endl;
		}

		while(inputfile >> i >> x >> y)
		{
			//map<int ,NodeInfo >::iterator iter = NodeInfoMap.find(i)
			NodeInfo temp(x,y);
			NodeInfoMap[i] = temp;
			if(CountNode < i)  CountNode = i;//获取数据中最大的节点下表
		}


		inputfile.close();

	}

	
	bool check(int nodeid)
	{
		if(NodeInfoMap[nodeid].x > 0.0)
			return true;
		return false;
	}
	//void printNodeLinkInfoMap()
	//{
	//	vector<list <int > >::iterator iter2 = NodeLinkInfoMap.begin();
	//	while(iter2 != NodeLinkInfoMap.end())
	//	{
	//		
	//		//for_each(iter2->second.begin(),iter2->second.end(),print);
	//		list<int>::iterator liter = iter2->second.begin();
	//		while(liter != iter2->second.end())
	//		{
	//			cout << *liter++ ;

	//		}
	//		cout <<endl;
	//		iter2 ++;
	//	}
	//}
	bool  isTarget(int nodei, int endnode)
	{
		if(nodei == endnode)
			return true;
		return false;
	}
	
	void init()
	{
		closeSet.erase(closeSet.begin(),closeSet.end());
		openSet.erase(openSet.begin(),openSet.end());
		ParentNode.erase(ParentNode.begin(),ParentNode.end());
		ParentNode.resize(CountNode);
		
		RoadPath.erase(RoadPath.begin(),RoadPath.end());
		Path.erase(Path.begin(),Path.end());

		maxCost = 1.414* getCost(StartNode,EndNode);
#ifdef YOUHUA1
	getRegion();

#endif
	



	}

	//这个用来扩展从该节点可以到达的所有结果
	void extendRoad(int nodei)
	{
		
		if (NodeLinkInfoMap[nodei].count == 0)
		{
			return ;
		}

		list<int>::iterator liter = NodeLinkInfoMap[nodei].link.begin();

		map<int ,CostNode>::iterator openiter = openSet.find(nodei);
		map<int ,CostNode>::iterator closeiter ;
		CostNode prenode(0,0,0); 
		if(openiter  == openSet.end())
		{
			prenode.toCostNode(nodei,getCost(nodei,StartNode),getCost(nodei,EndNode));
		}
		else 
			prenode = openiter->second ;


		for(; liter != NodeLinkInfoMap[nodei].link.end() ;  liter ++)
		{
			int tnode = *liter;

			if(isInRegion(tnode)  == false)
				continue;
			CostNode tmpNode(nodei,prenode.dist + getCost(nodei,tnode),prenode.dist + getCost(nodei,tnode)+ getCost(tnode,EndNode));
			//CostNode tmpNode(nodei,prenode.dist + getCost(nodei,tnode),getCost(nodei,tnode)+ getCost(tnode,EndNode));
			if(closeSet.find(tnode)  == closeSet.end())
			{
				openiter = openSet.find(tnode);
				if(openiter == openSet.end() ){
					
					//if(isInRegion(tnode) == true){
					//	openSet.insert( map<int ,CostNode>::value_type(tnode,tmpNode));
					//	ParentNode[tnode] = nodei;
					//}
					//else
						//closeSet.insert(map<int,CostNode>::value_type(tnode,tmpNode));

					openSet.insert( map<int ,CostNode>::value_type(tnode,tmpNode));
					ParentNode[tnode] = nodei;
				}
				else //如果 tnode 在openSet 中已经存在
				{
					//如果原来的花费 要大于现在的花费 ，那么就需要更新
					if(openiter->second.sum  > tmpNode.sum ){
						openiter->second.dist = tmpNode.dist;
						openiter->second.sum = tmpNode.sum;
						openiter->second.p = tmpNode.p;
						ParentNode[tnode] = nodei;
					}
				}				
			}
			else
			{
				//如过tnode 在closeSet 中，那如果tnode 要由于之前的closeSet ，那么把这个节点从closeset 中删除，并加入到openSet 中去
				closeiter = closeSet.find(tnode);
				if(tmpNode.sum < closeiter->second.sum  ){
					closeSet.erase(closeiter);
					openSet.insert(map<int ,CostNode>::value_type(tnode,tmpNode) );
					ParentNode[tnode] = nodei;
				}
				
			}
			//addIncloseSet(tnode);


		}//end for
	}

	double getCost(int nodei ,int enode,int condition =2)
	{
		NodeInfo inodeInfo = NodeInfoMap[nodei];
		NodeInfo jnodeInfo = NodeInfoMap[enode];
		double i_x = inodeInfo.x ;double i_y = inodeInfo.y;
		double j_x = jnodeInfo.x ;double j_y= jnodeInfo.y;
		if(condition == 1)
		{

			return sqrt((i_x-j_x)*(i_x-j_x) + (i_y-j_y)*(i_y-j_y));
		}
		else if(condition == 2)
		{
			return fabs(i_x -j_x) + fabs(i_y-j_y);
		}
		

	}
	int getBest()
	{
		int besti = 0;
		double bestvalue  = 999999999.9;

		map<int ,CostNode>::iterator iter = openSet.begin();
		for( ; iter != openSet.end() ; iter ++)
		{
			if(iter->second.sum  < bestvalue)
			{
				bestvalue = iter->second.sum;
				besti = iter->first;
			}
		}
		return besti;		
	}





	list<int> getPath(int enode)
	{

	}

	list<int> makePath(int inode)
	{
		int pre = inode;
		Path.push_front(pre);
		while(pre != StartNode)
		{
			int now = ParentNode[pre];
			Path.push_front(now);

			char str[20] = {NULL};
			sprintf(str,"%d+%d",now,pre);
			string strRoad(str);
			int roadid = startMapRoad.find(strRoad)->second;
			RoadPath.push_front(roadid);

			pre = now;
		}
		return Path;
	}
	
	double getRoadLen(int nodeId)
	{
		//list<int >subPath;
		int pre = nodeId;
		double len = 0.0;
		while(pre != StartNode)
		{
			int now = ParentNode[pre];
			char str[20] = {NULL};
			sprintf(str,"%d+%d",now,pre);
			string strRoad(str);
			int roadid = startMapRoad.find(strRoad)->second;
			len += RoadInfoMap[roadid].Length;

		}
		
		return len;

	}

	void addIncloseSet(int nodeId)
	{
		map<int ,CostNode>::iterator iter = openSet.find(nodeId);
		if(iter == openSet.end())
			return ;
		
		//double roadLen = getRoadLen(nodeId);
		if(  (isInRegion(nodeId) == false) || iter ->second.sum  > maxCost)
		{
			closeSet.insert(map<int,CostNode>::value_type(nodeId,iter->second));
			openSet.erase(iter);

		}

	}

#ifdef YOUHUA1

	void getRegion()
	{

		//weigh = 0.001;
		weigh = 0.0002;
		maxX = max(NodeInfoMap[StartNode].x,NodeInfoMap[EndNode].x) + weigh;
		maxY = max(NodeInfoMap[StartNode].y,NodeInfoMap[EndNode].y) + weigh;
		minX = min(NodeInfoMap[StartNode].x,NodeInfoMap[EndNode].x) - weigh;
		minY = min(NodeInfoMap[StartNode].y,NodeInfoMap[EndNode].y) - weigh;

	}

	bool isInRegion(int nodeId)
	{
		
		double x = NodeInfoMap[nodeId].x;
		double y = NodeInfoMap[nodeId].y;


		if( x < maxX && x > minX  && y < maxY  && y > minY)
			return true;
		return false;
	}


#endif
};
